#include "./BSP/MPU6050/MPU6050.h"

#define MPU6050_ADDRESS		0xD0		//MPU6050的I2C从机地址

typedef struct MPU6050_InitTypedef
{
	uint16_t SMPLRT_Rate;              //采样率Hz
	Filter_Typedef Filter;             //滤波器
	GYRO_CONFIG_Typedef gyro_range;    //陀螺仪测量范围
	ACCEL_CONFIG_Typedef acc_range;    //加速度计测量范围
	FIFO_EN_Typedef FIFO_EN;           //FIFO缓冲区使能
	INT_EN_Typedef INT;                //中断使能
}MPU6050_InitTypedef;

const static float dt = 0.01f;// 采样时间间隔(s),注意和你的采样率密切相关，你的采样率是多少这里就改为对应的时间

/**
  * 函    数：MPU6050写寄存器
  * 参    数：RegAddress 寄存器地址，范围：参考MPU6050手册的寄存器描述
  * 参    数：Data 要写入寄存器的数据，范围：0x00~0xFF
  * 返 回 值：无
  */
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
	MPU6050_IIC_Start();						//I2C起始
	MPU6050_SendByte(MPU6050_ADDRESS);	//发送从机地址，读写位为0，表示即将写入
	MPU6050_ReceiveAck();					//接收应答
	MPU6050_SendByte(RegAddress);			//发送寄存器地址
	MPU6050_ReceiveAck();					//接收应答
	MPU6050_SendByte(Data);				//发送要写入寄存器的数据
	MPU6050_ReceiveAck();					//接收应答
	MPU6050_IIC_Stop();						//I2C终止
}

/**
  * 函    数：MPU6050读寄存器
  * 参    数：RegAddress 寄存器地址，范围：参考MPU6050手册的寄存器描述
  * 返 回 值：读取寄存器的数据，范围：0x00~0xFF
  */
uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
	uint8_t Data;
	
	MPU6050_IIC_Start();					//I2C起始
	MPU6050_SendByte(MPU6050_ADDRESS);	//发送从机地址，读写位为0，表示即将写入
	MPU6050_ReceiveAck();					//接收应答
	MPU6050_SendByte(RegAddress);			//发送寄存器地址
	MPU6050_ReceiveAck();					//接收应答
	
	MPU6050_IIC_Start();						//I2C重复起始
	MPU6050_SendByte(MPU6050_ADDRESS | 0x01);	//发送从机地址，读写位为1，表示即将读取
	MPU6050_ReceiveAck();					//接收应答
	Data = MPU6050_Receive();			//接收指定寄存器的数据
	MPU6050_SendAck(1);					//发送应答，给从机非应答，终止从机的数据输出
	MPU6050_IIC_Stop();						//I2C终止

	return Data;
}
//软件校准
static int16_t gyro_zero_z = 0 ;
static void MPU6050_SoftCalibrate_Z(uint16_t calibrate_count)
{
	MPU6050_Data sensor_data;
	int32_t gyro_z_sum = 0;
	for (uint16_t i = 0; i < calibrate_count; i++)
	{
		MPU6050_getraw(&sensor_data);
		gyro_z_sum += sensor_data.Gyro.Z;
		delay_ms(10);
	}
	gyro_zero_z = gyro_z_sum / calibrate_count;
	delay_ms(100);
}

static void MPU6050_Register_init(MPU6050_InitTypedef *mpu6050_initTypedef)
{
	MPU6050_WriteReg(PWR_MGMT_1, 0x80);		//复位
	delay_ms(100);
	MPU6050_WriteReg(PWR_MGMT_1, 0x00);		//唤醒，设置时钟源为内部8MHz
	uint8_t SMPLRT_DIV;
	if (mpu6050_initTypedef->SMPLRT_Rate > 1000) mpu6050_initTypedef->SMPLRT_Rate = 1000;
	else if (mpu6050_initTypedef->SMPLRT_Rate < 4) mpu6050_initTypedef->SMPLRT_Rate = 4;
	SMPLRT_DIV = 1000 / mpu6050_initTypedef->SMPLRT_Rate - 1;
	MPU6050_WriteReg(MPU_SAMPLE_RATE_REG, SMPLRT_DIV);	//设置采样率
	MPU6050_WriteReg(MPU_INT_EN_REG,mpu6050_initTypedef->INT);	//中断使能
	MPU6050_WriteReg(MPU_CFG_REG,mpu6050_initTypedef->Filter);	//配置滤波器
	MPU6050_WriteReg(GYRO_CONFIG,mpu6050_initTypedef->gyro_range);	//陀螺仪测量范围
	MPU6050_WriteReg(ACCEL_CONFIG,mpu6050_initTypedef->acc_range);	//加速度计测量范围
	MPU6050_WriteReg(MPU_FIFO_EN_REG,mpu6050_initTypedef->FIFO_EN);	//FIFO
	uint8_t temp=0x00;
	if (mpu6050_initTypedef->FIFO_EN != 0x00)temp |= 0x40; //使能FIFO
	if (mpu6050_initTypedef->INT & 0x01) temp |= 0x08; //如果打开中断
	MPU6050_WriteReg(MPU_USER_CTRL_REG,temp);	//用户控制寄存器
	MPU6050_WriteReg(PWR_MGMT_1,0x01);	
}
/**
  * 函    数：MPU6050初始化
  * 参    数：无
  * 返 回 值：无
  */
void MPU6050_Init(void)
{
	MPU6050_IO_init();
	MPU6050_InitTypedef mpu6050_initTypedef;
	mpu6050_initTypedef.SMPLRT_Rate = 100;          //采样率50Hz
	mpu6050_initTypedef.Filter = Band_5Hz;     //低通滤波器5Hz
	mpu6050_initTypedef.gyro_range = gyro_250;   //陀螺仪量程500度每秒
	mpu6050_initTypedef.acc_range = acc_2g;     //加速度计量程4g
	mpu6050_initTypedef.FIFO_EN = FIFO_Disable;       //关闭FIFO
	mpu6050_initTypedef.INT = interrupt_Disable;               //关闭中断
	MPU6050_Register_init(&mpu6050_initTypedef); //寄存器初始化
	delay_ms(100);
	MPU6050_SoftCalibrate_Z(500); //软件校准
}

/**
  * 函    数：MPU6050获取ID号
  * 参    数：无
  * 返 回 值：MPU6050的ID号
  */
uint8_t MPU6050_GetID(void)
{
	return MPU6050_ReadReg(MPU_DEVICE_ID_REG);		//返回WHO_AM_I寄存器的值
}


void MPU6050_getraw(MPU6050_Data *sensor_data)
{
	sensor_data->Accel.X = (MPU6050_ReadReg(ACCEL_XOUT_H) << 8) | MPU6050_ReadReg(ACCEL_XOUT_L);
	sensor_data->Accel.Y = (MPU6050_ReadReg(ACCEL_YOUT_H) << 8) | MPU6050_ReadReg(ACCEL_YOUT_L);
	sensor_data->Accel.Z = (MPU6050_ReadReg(ACCEL_ZOUT_H) << 8) | MPU6050_ReadReg(ACCEL_ZOUT_L);
	sensor_data->Gyro.X = (MPU6050_ReadReg(GYRO_XOUT_H) << 8) | MPU6050_ReadReg(GYRO_XOUT_L);
	sensor_data->Gyro.Y = (MPU6050_ReadReg(GYRO_YOUT_H) << 8) | MPU6050_ReadReg(GYRO_YOUT_L);
	sensor_data->Gyro.Z = (MPU6050_ReadReg(GYRO_ZOUT_H) << 8) | MPU6050_ReadReg(GYRO_ZOUT_L);
}


//---------------------------------------------------------------卡尔曼滤波
typedef struct 
{
	float q;//过程噪声
	float r;//测量噪声
	float x;//状态
	float p;//估计误差
	float k;//卡尔曼增益
	/* data */
}MPU6050_KalmanFilter;

//卡尔曼滤波更新函数
static float MPU6050_KalmanFilter_Update(MPU6050_KalmanFilter *kf, float measurement)
{
	//预测更新
	kf->p = kf->p + kf->q;
	//计算卡尔曼增益
	kf->k = kf->p / (kf->p + kf->r);
	//更新估计值
	kf->x = kf->x + kf->k * (measurement - kf->x);
	//更新估计误差
	kf->p = (1 - kf->k) * kf->p;
	return kf->x;
}

//快速平方根算法
/*
static inline float fast_sqrt(float x)
{
	float xhalf = 0.5f * x;
	float y=x;
	long i = *(long*)&y; // get bits for floating VALUE
	i=0x5f3759df - (i>>1); // gives initial guess y0
	y=*(float*)&i;
	y=y*(1.5f - xhalf*y*y); // Newton step, repeating increases accuracy
	return y;
}*/
void MPU6050_Get_EulerAngle(EulerAngle* angle)
{
	float AX,AY,AZ,GX,GY,GZ;
	MPU6050_Data sensor_data;
	static MPU6050_KalmanFilter kf_roll,kf_pitch;
	static int one_flag=1;
	if (one_flag)
	{
		//roll初始化
		kf_roll.q = 0.001f;
		kf_roll.r = 0.1f;
		kf_roll.x=0;
		kf_roll.p=1;
		/* code */
		//Pitch初始化
		kf_pitch.q = 0.001f;
		kf_pitch.r = 0.1f;
		kf_pitch.x=0;
		kf_pitch.p=1;
		one_flag=0;
	}
	MPU6050_getraw(&sensor_data);
	AX = sensor_data.Accel.X*2.0f/32768;
	AY = sensor_data.Accel.Y*2.0f/32768;
	AZ = sensor_data.Accel.Z*2.0f/32768;
	GX = sensor_data.Gyro.X*dt*0.0174532f;
	GY = sensor_data.Gyro.Y*dt*0.0174532f;
	GZ = (sensor_data.Gyro.Z-gyro_zero_z)*dt*0.0174532f;

	// 计算加速度的绝对值
    float absAcc = sqrt(AX * AX + AY * AY + AZ * AZ);
    //ACC_abs = absAcc;
    // 动态调整权重
    float weight;
    if (absAcc > 1.2) {
        // 快速运动或剧烈振动状态，减小加速度计权重
        weight = 0.8f;
    } else {
        // 正常运动状态，强烈信任加速度计
        weight = 1.0f;
    }

	static float Gyroscope_roll =0.0f;
	static float Gyroscope_pitch =0.0f;

	Gyroscope_roll += GY;
	Gyroscope_pitch += GX;

	float raw_roll=weight * (atan2(AY, AZ)/3.1415926f *180.f) + (1 - weight) * Gyroscope_roll;
	float raw_pitch=-(weight * (atan2(AX, AZ)/3.1415926f *180.f) + (1 - weight) * Gyroscope_pitch);

	angle->Roll = MPU6050_KalmanFilter_Update(&kf_roll, raw_roll);
	angle->Pitch = MPU6050_KalmanFilter_Update(&kf_pitch, raw_pitch);
	if (fabsf(GZ)>=0.01)
	{
		angle->Yaw += GZ/2*4;
		/* code */
	}
}
